#ifndef LIDAR_LOCALIZATION_LOCALIZATION_FLOW_HPP_
#define LIDAR_LOCALIZATION_LOCALIZATION_FLOW_HPP_

#include <ros/ros.h>
// subscriber
#include "lidar_localization/subscriber/cloud_subscriber.hpp"
#include "lidar_localization/subscriber/imu_subscriber.hpp"
#include "lidar_localization/subscriber/odometry_subscriber.hpp"
#include "lidar_localization/tf_listener/tf_listener.hpp"
// publisher
#include "lidar_localization/publisher/cloud_publisher.hpp"
#include "lidar_localization/publisher/odometry_publisher.hpp"
#include "lidar_localization/publisher/tf_broadcaster.hpp"
// models
#include "lidar_localization/models/scan_adjust/distortion_adjust.hpp"
#include "lidar_localization/matching/matching.hpp"
#include "lidar_localization/models/graph_optimizer/g2o/g2o_graph_optimizer.hpp"
namespace lidar_localization
{
  class LocalizationFlow
  {
  public:
    LocalizationFlow(ros::NodeHandle &nh);

    bool Run();

  private:
    bool ReadData();
    bool InitCalibration();
    bool InitPose();
    bool ValidData();
    bool PublishData();
    bool SyncData(bool inited);
    bool Fuse();
    void Marginalize(bool first_key_frame);
    bool SavePose(std::ofstream& ofs, const Eigen::Matrix4f& pose);

  private:
    // subscriber
    std::shared_ptr<CloudSubscriber> cloud_sub_ptr_;
    std::shared_ptr<IMUSubscriber> imu_sub_ptr_;
    std::shared_ptr<OdometrySubscriber> laser_odom_sub_ptr_;
    std::shared_ptr<OdometrySubscriber> gnss_pose_sub_ptr_;
    std::shared_ptr<TFListener> lidar_to_imu_ptr_;
    // publisher
    std::shared_ptr<CloudPublisher> global_map_pub_ptr_;
    std::shared_ptr<CloudPublisher> local_map_pub_ptr_;
    std::shared_ptr<CloudPublisher> current_scan_pub_ptr_;
    std::shared_ptr<OdometryPublisher> laser_localization_pub_ptr_;
    std::shared_ptr<TFBroadCaster> laser_tf_pub_ptr_;
    // models
    std::shared_ptr<Matching> matching_ptr_;

    Eigen::Matrix4f lidar_to_imu_ = Eigen::Matrix4f::Identity();

    std::deque<CloudData> cloud_data_buff_;
    std::deque<PoseData> gnss_pose_data_buff_;
    std::deque<PoseData> laser_odom_data_buff_;
    std::deque<IMUData> imu_data_buff_;

    PoseData current_gnss_pose_data_;
    PoseData current_laser_odom_data_;
    CloudData current_cloud_data_;
    std::deque<IMUData> current_imu_data_;
    PoseData key_laser_odom_data_;

    State state_;
    State current_pose_;
    Eigen::VectorXd odom_edge_noise_;
    Eigen::VectorXd matching_noise_;
    Eigen::VectorXd imu_noise_;
    Eigen::Vector3d gravity_;
    double key_distance_threshold_;
    double key_time_threshold_;
    int window_size_;
    int key_frame_cnt_;
    std::shared_ptr<InterfaceGraphOptimizer> graph_optimizer_ptr_;
    std::ofstream ground_truth_ofs_;
    std::ofstream localization_ofs_;
  };
} // namespace lidar_localization

#endif